#!/usr/bin/python
#!coding=utf-8

import socket
import json
import copy
import time
import yaml
import os
import traceback

import numpy as np

# rospy相关库
import rospy
from acupoint_msgs.msg import AcupointCoordinate,LegCoordinate
from std_msgs.msg import Bool
from std_msgs.msg import Int64,String, Float32


# Aruco码相对于JAKAZU7基座的齐次变换矩阵
M=np.matrix([[0,    -1, 0,  -9.5],
             [1,    0,  0,  -408.5],
             [0,    0,  1,  0],
             [0,    0,  0,  1]])

# JAKAZU7基座相对于Aruco码的齐次变换矩阵
inv_M=np.matrix([[0,    1,  0,   408.5],
                 [-1,   0,  0,   -9.5],
                 [0,    0,  1,   0],
                 [0,    0,  0,   1]])


RAD2DEG=180/3.1415926
DEG2RAD=3.1415926/180

# NUC服务器IP地址以及端口好
NUC_IP="192.168.1.100"
NUC_PORT=56789


# NUC_COMMUNICATE根目录
ROOT_PATH=os.path.dirname(os.path.abspath(__file__))


# AGV位点标定文件
AGV_POSITION_YAML=ROOT_PATH+"/config/agv_position.yaml"

# 按摩手法序列文件
MASSAGE_YAML=ROOT_PATH+"/config/sxy.yaml"
MASSAGE_YAML2=ROOT_PATH+"/config/user.yaml"
FLAG_YAML=False

class NucCommunicate:
    def __init__(self):
        
        # # 加载按摩手法序列文件
        # temp=self.loadYaml(MASSAGE_YAML)
        # if temp is None:
        #     print("Error, load yaml file failed")
        #     return
        # self.actions=temp['actions']
        
        # 加载标定AGV位点文件
        self.agv_postions=self.parseAgvposition()
        if self.agv_postions is None:
            print("Error, load yaml file failed")
            return
        
        print("parse completed")

        
        self.flag=-1
        
        # 穴位名以及穴位坐标
        self.acupoint_name=None
        self.acupoint_coordinate=None
        self.left_coordinate=[0,0,0]
        self.right_coordinate=[0,0,0]
        self.old_acupoint_coordinate=None
        
        
        # 穴位坐标订阅
        self.acupoint_sub = rospy.Subscriber("/acupoint/acu2base_coordinate", AcupointCoordinate, self.acupointCallback) 
        self.left_leg_sub = rospy.Subscriber("/acupoint/left_leg_line", LegCoordinate, self.leftLegCallback) 
        self.right_leg_sub = rospy.Subscriber("/acupoint/right_leg_line", LegCoordinate, self.rightLegCallback) 
        
        # 按摩中穴位发布
        self.work_acupoint_pub=rospy.Publisher("/working_acupoint",String,queue_size=1)
        
        # AGV Driver通讯topic
        self.agv_pub=rospy.Publisher("/agv/agv_position",Int64,queue_size=1)
        self.agv_sub=rospy.Subscriber("/agv/agv_arrive", Bool, self.agvCallback) 
        
        # yaml通讯topic
        #self.yaml_pub=rospy.Publisher("",Int64,queue_size=1)
        self.yaml_sub=rospy.Subscriber("/massage/yaml_flag", Bool, self.yamlCallback) 

        # acupoint text 发布
        self.text_method_pub = rospy.Publisher("/Acupoint/method",String,queue_size=10)
        self.text_name_pub = rospy.Publisher("/Acupoint/name",String,queue_size=10)
        self.text_time_pub = rospy.Publisher("/Acupoint/time",String,queue_size=10)

        # 连接服务器
        print("connnect to the server")
        self.client=socket.socket(socket.AF_INET,socket.SOCK_STREAM)
        self.client.connect((NUC_IP,NUC_PORT))
        
        # 等待yaml
        print("wait for loading YAML file")
        global FLAG_YAML
        while not rospy.is_shutdown() and not FLAG_YAML:
            print("wait yaml")
            rospy.sleep(0.1)

        # 调试用
        # yamlPath = "./config/user.yaml"
        # # 加载按摩手法序列文件
        # temp=self.loadYaml(yamlPath)
        # if temp is None:
        #     print("Error, load yaml file failed")
        #     return
        # self.actions=temp['actions']
        
        if not rospy.is_shutdown():
            # JAKA ZU7初始化
            data={'method':10}
            self.sendCommandToJaka(data)   
            
            # AGV初始化     
            self.agv_move_finish_flag=False
            self.agv_point_index=-1
            self.agvMoveToInitPoint()

            # 等待穴位topic
            print("wait for acupoint information")
            while not rospy.is_shutdown() and (self.acupoint_coordinate is None or self.left_coordinate[0]==0 or self.right_coordinate[0]==0):
                rospy.sleep(0.1)

            # 机器人开始按摩
            print("massage action start")
            try:   
                # 按摩序列分割
                massage_actions=[]   
                try:
                    temp=[]
                    for action in self.actions:
                        method=action['method']
                        if method==-1:
                            massage_actions.append(temp)
                            temp=[]
                        else:
                            temp.append(action)
                    
                    # 处理切割后的每段按摩子序列      
                    for massage_action in massage_actions:
                        self.agvMove(massage_action)
                        print('agvmoved=>jakamove')
                        self.jakaMove(massage_action)
                        print('jakamoved')
                    
                    # jaka 復原
                    data={'method':10}
                    self.sendCommandToJaka(data)
                except:
                    print("error during actions")
                    print(traceback.format_exc())
            except:
                print("connect failed")
                print(traceback.format_exc())
            finally:
                self.client.close()
    
    
    # 控制AGV运动到初始点
    def agvMoveToInitPoint(self):
        print("move agv to init point")
        self.agvMoveToSelectPoint(0)
        print("agv has move to init point")
        # print("move agv to start point")
        # self.agvMoveToSelectPoint(2)
        # print("agv has move to start point")
        
     
    # 控制AGV运动到指定位点
    def agvMoveToSelectPoint(self,agv_index):
        print("move agv to the select point %d" % agv_index)
        #wait_key = raw_input("ensure agv move >")
        wait_key = 'y'
        if wait_key=='y':
            self.agv_move_finish_flag=False
            agv_msg=Int64()
            agv_msg.data=agv_index+1
            self.agv_pub.publish(agv_msg)
            while not rospy.is_shutdown() and not self.agv_move_finish_flag:
                rospy.sleep(0.1)
            self.agv_move_finish_flag=False
            self.agv_point_index=agv_index
            print("agv has move to the select %d point" % agv_index)
            
            # 设置AGV位点到ROS参数服务器中，主要用于ARUCO码被遮挡时直接读取提前标定好的ARUCO码相对于相机的齐次变换矩阵
            rospy.set_param('/agv/agv_point_index',agv_index)
        else:
            pass
    
    # 给JAKA发送控制指令
    def sendCommandToJaka(self,data):
        data_json=json.dumps(data)
        print("the command sent to the jaka is: ",data_json)
        # wait_key = raw_input("ensure jaka move >")
        wait_key = 'y'
        if wait_key=='y':
            self.client.send(data_json.encode())
            # 避免机器人运动时间过长导致TCP超时
            self.client.settimeout(600)
            recv_data=self.client.recv(1024)
            if not recv_data:
                return
            jaka_arrived_msg=Bool()
            jaka_arrived_msg=True
            #self.jaka_arrive_pub.publish(jaka_arrived_msg)
        else:
            pass
        
    # 计算其次变换矩阵的逆
    # parameter:
    #     M: type np.matrix
    # return: 
    #     inv_T: type np.matrix
    def getInverseMatrix(self,M):
        inv_T=np.matrix([[1,0,0,0],
                         [0,1,0,0],
                         [0,0,1,0],
                         [0,0,0,1.0]])
        inv_T[0:3,0:3]=M[0:3,0:3].T
        inv_T[0:3,3]=-M[0:3,0:3].T*M[0:3,3]
        return inv_T
       
        
    # 解析AGV所有点位相对于相机坐标系的相对关系
    # parameter:
    #     None
    # return: 
    #     agv_positions: type list
    def parseAgvposition(self):
        temp=self.loadYaml(AGV_POSITION_YAML)
        if temp is None:
            print("Error, load agv position config file failed")
            return []
        
        positions=temp['agv_points']
        agv_positions=[]
        for position in positions:
            agv_positions.append(self.quantern2RotationMatrix(position))
        return agv_positions
      
      
    # 解析出AGV点位相对于相机坐标系的相对关系
    # parameter:
    #     point: type geometry_msgs/Pose 
    # return: 
    #     Transpose_Matrix: type np.matrix  
    def quantern2RotationMatrix(self, point):
        x=point['orientation']['x']
        y=point['orientation']['y']
        z=point['orientation']['z']
        w=point['orientation']['w']
        M=np.matrix([[1-2*y*y-2*z*z,   2*x*y+2*w*z,      2*x*z-2*w*y,    point['position']['x']],
                           [2*x*y-2*w*z,     1-2*x*x-2*z*z,    2*y*z+2*w*x,    point['position']['y']],
                           [2*x*z+2*w*y,     2*y*z-2*w*x,      1-2*x*x-2*y*y,  point['position']['z']],
                           [0,               0,                0,              1]]);
        return M
        
     
    # 解析出AGV其他点位相对于AGV目前位置的相对旋转矩阵
    # parameter:
    #     None
    # return: 
    #     other_position: type list   
    def getOtherAgvPosition(self):
        other_position=[]
        position_now=self.getInverseMatrix(self.agv_postions[self.agv_point_index])
        for position in self.agv_postions:
            temp=M*position_now*position*inv_M
            other_position.append([temp[0,3],temp[1,3],temp[2,3]])
        return other_position
    

    # 控制JAKA运动
    def jakaMove(self,massage_action):
        while not rospy.is_shutdown() and (self.acupoint_coordinate is None or self.left_coordinate[0]==0 or self.right_coordinate[0]==0):
            # print('acupoint_pose',self.acupoint_coordinate)
            # print('left',self.left_coordinate)
            # print('right',self.right_coordinate)
            rospy.sleep(0.1)
        x=-1
        y=-1
        z=-1
        action_index=0
        # print("begin") start massage on acupoint !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        while action_index<len(massage_action):
            action=massage_action[action_index]
            redo_flag=False
            method=action['method']

            #发布数据至GUI
            acuName = String()
            acuMethod = String()
            acuTime = String()
            
            if method == 3:
                acuMethod.data = '捏法'
                self.text_method_pub.publish(acuMethod)
            elif method == 2:
                acuMethod.data = '振法'
                self.text_method_pub.publish(acuMethod)
            elif method == 1:
                acuMethod.data = '揉法'
                self.text_method_pub.publish(acuMethod)
            elif method == 0:
                acuMethod.data = '滚法'
                self.text_method_pub.publish(acuMethod)
            elif method == -1:
                acuMethod.data = '复原'
                self.text_method_pub.publish(acuMethod)

            acuName.data = action['name']
            acuTime.data = str(action['time'])
            print(type(action['time']))
            self.text_name_pub.publish(acuName)
            self.text_time_pub.publish(acuTime)

            if method==3:
                if action['name']=='left_leg':
                    x=self.left_coordinate[0]
                    y=self.left_coordinate[1]
                    z=self.left_coordinate[2]
                elif action['name']=='right_leg':
                    x=self.right_coordinate[0]
                    y=self.right_coordinate[1]
                    z=self.right_coordinate[2]
                else:
                    print("undefined name in tweak")
                # 当识别出的坐标不对时，重新开始识别
                # if x<-680:
                #     redo_flag=True
            else:
                index=self.acupoint_name.index(action['name'])
                x=self.acupoint_coordinate[index].x
                y=self.acupoint_coordinate[index].y
                z=self.acupoint_coordinate[index].z
                # Trick，当识别出的穴位Z坐标过高时（穴位高度估算出问题，即机械臂遮挡住穴位），直接读取历史穴位数据
                if z>300:
                    x=self.old_acupoint_coordinate[index].x
                    y=self.old_acupoint_coordinate[index].y
                    z=self.old_acupoint_coordinate[index].z
                # Trick，如果识别出的穴位数据不对，则直接重新识别（穴位数据存在波动时）
                # if x<-680: 
                #     redo_flag=True
                    
            # 如果不要求重新识别，则将按摩工作序列发送给机器人
            if not redo_flag:
                work_acupoint_msg=String()
                work_acupoint_msg.data=action['name']
                self.work_acupoint_pub.publish(work_acupoint_msg)
                
                data={'method':action['method'],'time':action['time'],'x':x,'y':y,'z':z}
                self.sendCommandToJaka(data)
                action_index=action_index+1
        work_acupoint_msg=String()
        work_acupoint_msg.data="end"
        self.work_acupoint_pub.publish(work_acupoint_msg)
    
    # 解析按摩穴位相对于机器人底座的坐标
    def getMassageCoordinate(self,massage_action):
        coordinates=[]
        x_list=[]
        y_list=[]
        z_list=[]
        for action in massage_action:
            method=action['method']
            # 滚法
            if method==0:
                index=self.acupoint_name.index(action['name'])
                x=self.acupoint_coordinate[index].x
                y=self.acupoint_coordinate[index].y
                z=self.acupoint_coordinate[index].z+50
                rx=-90*DEG2RAD
                ry=0*DEG2RAD
                rz=90*DEG2RAD
                x_list.append(x)
                y_list.append(y)
                z_list.append(z)
                coordinates.append([x,y,z,rx,ry,rz])
            # 揉法
            elif method==1:
                index=self.acupoint_name.index(action['name'])
                x=self.acupoint_coordinate[index].x-40
                y=self.acupoint_coordinate[index].y+30
                z=self.acupoint_coordinate[index].z+150
                rx=90*DEG2RAD
                ry=0*DEG2RAD
                rz=-90*DEG2RAD
                x_list.append(x)
                y_list.append(y)
                z_list.append(z)
                coordinates.append([x,y,z,rx,ry,rz])
            # 振法
            elif method==2:
                index=self.acupoint_name.index(action['name'])
                x=self.acupoint_coordinate[index].x-60
                y=self.acupoint_coordinate[index].y+40
                z=self.acupoint_coordinate[index].z+125
                rx=-90*DEG2RAD
                ry=-90*DEG2RAD
                rz=90*DEG2RAD
                x_list.append(x)
                y_list.append(y)
                z_list.append(z)
                coordinates.append([x,y,z,rx,ry,rz]) 
            # 捏法
            elif method==3:
                if action['name']=='left_leg':
                    x=self.left_coordinate[0]
                    y=self.left_coordinate[1]
                    z=self.left_coordinate[2]
                elif action['name']=='right_leg':
                    x=self.right_coordinate[0]
                    y=self.right_coordinate[1]
                    z=self.right_coordinate[2]
                else:
                    print("undefined name in tweak")
                x_list.append(x)
                y_list.append(y)
                z_list.append(z)  
            else:
                print("undefined massage method")
        return coordinates,x_list,y_list,z_list
     
        
    # AGV控制
    def agvMove(self,massage_action):
        coordinates,x_list,y_list,z_list=self.getMassageCoordinate(massage_action)
        
        x_min=min(x_list)
        x_max=max(x_list)
        y_min=min(y_list)
        y_max=max(y_list)
        z_min=min(z_list)
        z_max=max(z_list)
        x=(x_min+x_max)/2
        y=(y_min+y_max)/2
        z=(z_min+z_max)/2     
        print("the center point is:",x,y,z)
        
        # 解算出其他位点相对于目前位点的位置
        agv_positions=self.getOtherAgvPosition()
        
        
        # 选择最佳的位点
        min_distance=0.9*(x*x+y*y+z*z)
        min_index=self.agv_point_index
        for i in range(len(agv_positions)):
            d=(agv_positions[i][0]-x)**2+(agv_positions[i][1]-y)**2+(agv_positions[i][2]-z)**2
            if d<min_distance:
                min_index=i
                min_distance=d
        
        print("the choose agv point is:",min_index)
        
        # 如果最佳位点与机器人目前位点不同，则控制AGV运动到最佳位点
        if min_index!=self.agv_point_index: 
            data={'method':10}
            self.sendCommandToJaka(data)            
            self.agvMoveToSelectPoint(min_index)
            self.old_acupoint_coordinate=None
            self.acupoint_coordinate=None
            
    # AGV执行完毕回调函数
    def agvCallback(self,msg):
        self.agv_move_finish_flag=msg.data      
    
    # 穴位坐标回调函数 
    def acupointCallback(self,msg):
        self.acupoint_name=copy.deepcopy(msg).name
        self.acupoint_coordinate=copy.deepcopy(msg).acupoints
        if self.old_acupoint_coordinate is None:
            self.old_acupoint_coordinate=copy.deepcopy(msg).acupoints
    
    # 左腿穴位回调函数
    def leftLegCallback(self,msg):
        self.left_coordinate[0]=(msg.position[0].x+msg.position[1].x)/2
        self.left_coordinate[1]=(msg.position[0].y+msg.position[1].y)/2
        self.left_coordinate[2]=(msg.position[0].z+msg.position[1].z)/2
    
    # 右腿穴位回调函数
    def rightLegCallback(self,msg):
        self.right_coordinate[0]=(msg.position[0].x+msg.position[1].x)/2
        self.right_coordinate[1]=(msg.position[0].y+msg.position[1].y)/2
        self.right_coordinate[2]=(msg.position[0].z+msg.position[1].z)/2
    
    # yaml callback function
    def yamlCallback(self, msg):
        global FLAG_YAML
        if msg.data == True:
            if not FLAG_YAML:
                yamlPath = "./config/user.yaml"
                # 加载按摩手法序列文件
                temp=self.loadYaml(yamlPath)
                if temp is None:
                    print("Error, load yaml file failed")
                    return
                self.actions=temp['actions']
                FLAG_YAML = True
                print("YMAL LOAD")
    
    # 加载Yaml文件 
    def loadYaml(self,file_path):
        if not os.path.exists(file_path):
            print("Error, %s not exists"%file_path)
            return
        yaml_data=None
        try:
            with open(file_path) as f:
                yaml_data=yaml.safe_load(f.read())
        except:
            print("Some error occured,the exception message is:")
            print(traceback.format_exc())
        return yaml_data


if __name__ == "__main__":
    rospy.init_node('nuc_communicate_node')
    nc=NucCommunicate()



    

    
